#include "Instancia.h"
#include <iostream>
#include <limits>
#include <stdlib.h>
#include <math.h>
#include "TFloat.h"


/* CHECKBIT: returns 1 or 0 if the bit at POS in I is set or not */
#define CHECKBIT(I, POS) (!!((I) & (1 << (POS))))

using namespace std;

Instancia::Instancia (int r, int p){
	this->_numRocas = r;
	this->_rocas = new TFloat [r][3];
	this->_precision = p;
}

void Instancia::print(){
	cout<<endl;
	cout<<endl;
	cout<<"Instancia: A = (" << this->_a[0].dbl() << ", " << this->_a[1].dbl() << ", " << this->_a[2].dbl() << ") ";
	cout<<" | B = (" << this->_b[0].dbl() << ", " << this->_b[1].dbl() << ", " << this->_b[2].dbl() << ") ";
	cout<< " | Valor Critico = " << this->_valorCritico.dbl() << " |" << endl;
	cout<<this->_numRocas << " rocas: ";
	for(int i=0; i<this->_numRocas; i++){
		cout<<"(";
		cout<< this->_rocas[i][0].dbl() << ", " << this->_rocas[i][1].dbl() << ", " << this->_rocas[i][2].dbl() << ")  ";
	}

	//Tolerancia de los Métodos Secante y Newton_Raphson:
	TFloat tolerancia = TFloat(0.0001, this->_precision);

	//Iteracciones Máximas para los Métodos Secante y Newton_Raphson:
	int itersMax = 100;
	cout << endl;
	cout << endl;
	cout << "Para cada método se pide una tolerancia de " << tolerancia.dbl() << " y un máximo de " << itersMax << " iteraciones posibles."<<endl;
	
    //--Metodo Secante--
	cout<<endl;
	cout<<endl;
	cout<<endl;
	cout<<"-----------METODO DE SECANTE-----------"<<endl;
	TFloat p_0 = TFloat(0, this->_precision);
	TFloat p_1 = TFloat(1, this->_precision);
	cout<<endl;
	try{
		secante(p_0, p_1, tolerancia, itersMax);
	}catch(int e){ //overflow
		cout<<"Cayó Superman."<<endl; 
	}
	cout<<endl;

	//--Metodo Newton_Raphson--
	cout<<endl;
	cout<<endl;
	cout<<endl;
	cout<<"-----------METODO DE NEWTON_RAPHSON-----------"<<endl;
	TFloat x_0_Newton = TFloat(0.5, this->_precision);
	cout<<endl;
	try{
		newton_raphson(x_0_Newton, tolerancia, itersMax);
	}catch(int e){ //overflow
		cout<<"Cayó Superman."<<endl;
	}
	cout<<endl;

}

TFloat* Instancia::f(TFloat t){
	TFloat *res = new TFloat[3];
	for(int i=0; i<3; i++){
	    res[i] = (this->_a[i]*t) + this->_b[i];
	}
	return res;
}

TFloat Instancia::aturdimiento(TFloat t){
    TFloat res = TFloat(0, this->_precision);
	TFloat uno = TFloat(1, this->_precision);

    TFloat* f = this->f(t);
    for(int i=0; i < (this->_numRocas); i++){
        TFloat a = TFloat(pow((f[0] - this->_rocas[i][0]).dbl(),2), this->_precision);
		TFloat b = TFloat(pow((f[1] - this->_rocas[i][1]).dbl(),2), this->_precision);
		TFloat c = TFloat(pow((f[2] - this->_rocas[i][2]).dbl(),2), this->_precision);
		
        TFloat norma = TFloat(sqrt(a.dbl() + b.dbl() + c.dbl()),this->_precision);
		
		//CHECK FOR OVERFLOW POR SI LA NORMA ES MUY CHICA
		if ((uno/norma).dbl() > std::numeric_limits<double>::max()){
			throw 1;			
		}else{
	        res = res + (uno/norma);
		}		
    }
    return res;
    
}

//FuncionCero es la resta entre el valor Critico y la funcion de Aturdimiento
TFloat Instancia::funcionCero(TFloat t){	
	TFloat resul = TFloat(0.0, this->_precision);	
	resul = (this->_valorCritico - this->aturdimiento(t));
	return resul;
	}

//Derivada de funcion cero:
TFloat Instancia::funcionCeroDerivada(TFloat t){
	TFloat res = TFloat(0, this->_precision);
	
	TFloat a_0 = this->_a[0];
	TFloat a_1 = this->_a[1];
	TFloat a_2 = this->_a[2];
	
	TFloat* f = this->f(t);
	
	for(int i=0; i < (this->_numRocas); i++){
		TFloat f_Menos_Roca0 = TFloat(pow((f[0] - this->_rocas[i][0]).dbl(),2), this->_precision);
		TFloat f_Menos_Roca1 = TFloat(pow((f[1] - this->_rocas[i][1]).dbl(),2), this->_precision);
		TFloat f_Menos_Roca2 = TFloat(pow((f[2] - this->_rocas[i][2]).dbl(),2), this->_precision);

		TFloat norma = f_Menos_Roca0 + f_Menos_Roca1 + f_Menos_Roca2;
		TFloat normaAlCubo = norma*norma*norma;
		TFloat normaAlCuboRaiz = TFloat (sqrt(normaAlCubo.dbl()), this->_precision);
		
		TFloat a_0Xf_Menos_Roca0 = TFloat(a_0*f_Menos_Roca0);
		TFloat a_1Xf_Menos_Roca1 = TFloat(a_1*f_Menos_Roca1);
		TFloat a_2Xf_Menos_Roca2 = TFloat(a_2*f_Menos_Roca2);

		//CHECK FOR OVERFLOW 
		if (fabs(((a_0Xf_Menos_Roca0+a_1Xf_Menos_Roca1+a_2Xf_Menos_Roca2)/normaAlCuboRaiz).dbl()) > std::numeric_limits<double>::max() || isnan(((a_0Xf_Menos_Roca0+a_1Xf_Menos_Roca1+a_2Xf_Menos_Roca2)/normaAlCuboRaiz).dbl())){
			throw 1;
		}else{
			res = res+((a_0Xf_Menos_Roca0+a_0Xf_Menos_Roca0+a_0Xf_Menos_Roca0)/normaAlCuboRaiz);
		}
	}
	return res;
	
	}

TFloat Instancia::newton_raphson(TFloat inicial, TFloat tolerancia, int itersMax){
	int iter = 0;

	TFloat p_0 = inicial;
	TFloat p = p_0;

	while (iter < itersMax){		
		if(fabs(this->funcionCeroDerivada(p_0).dbl()) < tolerancia.dbl()){
			//p_0 ES EXTREMO, es máximo o mínimo local
			if ((funcionCero(p).dbl() < tolerancia.dbl())){
				cout<<"Solución del Método Newton_Raphson: "<< p.dbl()<< "  (en " << iter << " iteraciones)"<<endl;
				cout<<"Cayó Superman."<<endl;
				return p;
			}
		}
		if ((funcionCero(p).dbl() < tolerancia.dbl())){
			cout<<"Solución del Método Newton_Raphson: "<< p.dbl()<< "  (en " << iter << " iteraciones)"<<endl;
			cout<<"Cayó Superman."<<endl;
			return p;
		}
		//CHECK FOR OVERFLOW LA DIVISION POR LA DERIVADA MUY CHICA
		if (fabs((this->funcionCero(p_0) / this->funcionCeroDerivada(p_0)).dbl()) > std::numeric_limits<double>::max()){
			throw 1;
		}else{
	        //Sucesion de Newton:
			p = p_0 - (this->funcionCero(p_0) / this->funcionCeroDerivada(p_0));
		}

		iter++;
		p_0 = p;
	}
	cout<<"Método Newton_Raphson fracasó después de "<< iter << " iteraciones. Procedimiento terminado sin éxito."<<endl;
	cout<<"Superman vive!!"<<endl;
	return 0;
}


TFloat Instancia::secante(TFloat p_0, TFloat p_1, TFloat tolerancia, int itersMax){
	TFloat q_0 = TFloat(0, this->_precision);
	TFloat q_1 = TFloat(0, this->_precision);
	TFloat p = TFloat(0, this->_precision);

	q_0 = this->funcionCero(p_0);
	q_1 = this->funcionCero(p_1);
	
	int iter = 2;

	while (iter < itersMax){
		
		//CHECK FOR OVERFLOW 
		TFloat num = q_1*(p_1-p_0);
		TFloat div = q_1-q_0;
		if(div.dbl()<tolerancia.dbl()){
			iter++;
			p = ((p_1-p_0)/TFloat(4, this->_precision)*TFloat(1, this->_precision)) + p_0;
			p_0 = p_1;
			q_0 = q_1;
			p_1 = p;
			q_1 = this->funcionCero(p);	
			continue;
		}
		if ((num.dbl()/div.dbl()) > std::numeric_limits<double>::max()){	
			throw 1;
		}else{
	        //Sucesion de Secante:
			p = p_1 - ((q_1*(p_1-p_0))/(q_1-q_0));
			if((p.dbl()<tolerancia.dbl())||(p.dbl()>(tolerancia+TFloat(1,this->_precision)).dbl())){
				p = (p_0 + p_1)/TFloat(2, this->_precision);
			}
		}
	
		if ((funcionCero(p)).dbl() < tolerancia.dbl()){
			cout<<"Solución del Método Secante: "<< p.dbl()<< "  (en " << iter << " iteraciones)"<<endl;
			cout<<"Cayó Superman."<<endl;
			return p;
			}

		iter++;
		p_0 = p_1;
		q_0 = q_1;
		p_1 = p;
		q_1 = this->funcionCero(p);	
		
		}
	cout<<"Método Secante fracasó después de "<< iter << " iteraciones. Procedimiento terminado sin éxito."<<endl;	
	cout<<"Superman vive!!"<<endl;
	return 0;
}


//-----------Biseccion que NO es adecuada para este caso-----------
//En este método, nos cuesta mucho encontrar los casos donde f(a).f(b) < 0.

/*
TFloat Instancia::biseccion(TFloat tolerancia, int itersMax){

	TFloat mitad= TFloat(0.0, this->_precision);
	TFloat dos = TFloat(2.0, this->_precision);

	int iter = 0;

	TFloat a, b;
	a = TFloat(0.0, this->_precision);
	b = TFloat(1.0, this->_precision);

	TFloat *tmp = this->getValidInterval(a,b,tolerancia);
	if(tmp == NULL){
		cout<<"No hay raices."<<endl;
		return 0;
	}else{
		a = tmp[0];
		b = tmp[1];
	}

	//Consultamos si no superamos las cant. max de iteraciones posibles
	while (iter < itersMax){
	cout<<"["<<a.dbl()<<","<<b.dbl()<<"]"<<endl;
		mitad = (a+b)/dos;
		//Si estamos por debajo de la tolerancia o mitad ya es raiz terminamos
		if (((b-a)/dos).dbl() < tolerancia.dbl()){
			cout<<"Solución de Biseccion - La raiz es: "<< mitad.dbl()<<endl;
			cout<<endl;
			return mitad;
		}

		iter++;

		if ((this->funcionCeroBiseccion(a)*this->funcionCeroBiseccion(mitad)).dbl() < 0){
			b = mitad;
			}else{
			a = mitad;
		}
	}
	cout<<"Método Bisección fracasó después de "<< iter << " iteraciones. Procedimiento terminado sin éxito."<<endl;
	cout<<"La Mitad fue: "<<mitad.dbl()<<endl;
	cout<<"Funcion Cero en Mitad: "<<funcionCeroBiseccion(mitad).dbl()<<endl;
	cout<<"Tolerancia requerida: "<<tolerancia.dbl()<<endl;
	cout<<"(b-a)/2: "<<((b-a)/2).dbl()<<endl;
	cout<<endl;
	return 0;
}
*/

/*
TFloat* Instancia::getValidInterval(TFloat a, TFloat b, TFloat tolerancia){
	if (((b-a)).dbl() < tolerancia.dbl()){
		return NULL;
	}
	TFloat *res = new TFloat[2];
	if ((this->funcionCeroBiseccion(a)*this->funcionCeroBiseccion(b)).dbl() <= 0){
		res[0] = a;
		res[1] = b;
		return res;
	}

	TFloat dos = TFloat(2.0, this->_precision);
	TFloat mitad= (a+b)/dos;
	if ((this->funcionCeroBiseccion(a)*this->funcionCeroBiseccion(mitad)).dbl() <= 0){
		res[0] = a;
		res[1] = mitad;
		return res;
	}
	if ((this->funcionCeroBiseccion(mitad)*this->funcionCeroBiseccion(b)).dbl() <= 0){
		res[0] = mitad;
		res[1] = b;
		return res;
	}

	res = this->getValidInterval(a,mitad,tolerancia);
	if(res == NULL){
		res = this->getValidInterval(mitad,b,tolerancia);
	}

	return res;
}*/
